package org.noote.libs.spidy.comm;

import org.noote.libs.robot.helper.Helper_Robot;

import android.util.Log;

public class BluetoothCommandProtocol {

	protected static final int _PROTOCOL_VERSION_MAJOR=0;
	protected static final int _PROTOCOL_VERSION_MINOR=6;
	public static final String getProtocolVersion()
	{
		return Integer.toString(_PROTOCOL_VERSION_MAJOR)+"."+Integer.toString(_PROTOCOL_VERSION_MINOR);
	}
	
	//////////////////////////////////
	///
	/// Only for Spidy Arduino Sketch 0.6 !
	///
	/////////////////////////////////
	static final String CMD_HASH_TRAME = "*";
	static final String CMD_START_TRAME = "#";
	static final String CMD_END_TRAME = "$";
	protected static String _bounding(String sCmd)
	{
		byte hash = 0;
		for(Byte b : sCmd.getBytes()) hash += b.byteValue();
		String sTrame = CMD_HASH_TRAME+Integer.toString(hash)+CMD_START_TRAME+sCmd+CMD_END_TRAME;
		if(sTrame.length()>1024)
			Log.e("BluetoothCommandProtocol", "message to long : "+sTrame.length()+" bytes for max 1024 bytes");
		return sTrame;
	}
	
	/////////////////////////////////
	// Connect
	static final String CMD_CONNECT = "c";
	public static String sendAskConnection()
	{
		return _bounding(CMD_CONNECT);
	}
	
	/////////////////////////////////
	// Alive
	static final String CMD_ALIVE = "a";
	public static String sendAlive()
	{
		return _bounding(CMD_ALIVE);
	}
	
	/////////////////////////////////
	// Quit
	static final String CMD_QUIT = "q";
	public static String sendQuit()
	{
		return _bounding(CMD_QUIT);
	}

	/////////////////////////////////
	// Version
	static final String CMD_VERSION = "v";
	public static String sendAskVersion()
	{
		return _bounding(CMD_VERSION);
	}
	
	/////////////////////////////////
	// Servo
	static final String CMD_SERVO = "s";
	public static String sendAskAllServoPositions()
	{
		return _bounding(CMD_SERVO+"a");
	}
	public static String sendSetServoPosition(int idx, int pos)
	{
		return _bounding(CMD_SERVO+"s"+intToString(1)+intToString(idx)+intToString(Helper_Robot.servoDegreeToMicroSecond(pos)));
	}
	public static String sendSetServoPositions(int iOffset, int nServos, int []servos_id, float []servos_pos)
	{
		String build="";
		for(int i=0; i<nServos; i++)
		{
			build += intToString(servos_id[iOffset+i]);
			build += intToString(Helper_Robot.servoDegreeToMicroSecond(servos_pos[iOffset+i]));
		}
		return _bounding(CMD_SERVO+"s"+intToString(nServos)+build);
	}
	
	/////////////////////////////////
	// tools
	static String intToString(int i)
	{
		String s = Integer.toString(i);
		if(i>=0) s = "+"+s;
		return s;
	}
}
